import java.util.*;

public class UniformCostSearch {
	
	private static class Node implements Comparable<Node> {
		public StateWithCost state;
		public Node parent;
		public double pathCost;

		public Node(StateWithCost state, Node parent, double pathCost) {
			this.state = state;
			this.parent = parent;
			this.pathCost = pathCost;
		}

		public boolean equals(Object obj) {
			return state.equals(((Node)obj).state);
		}

		public int hashCode() {
			return state.hashCode();
		}

		public int compareTo(Node node) {
			if (pathCost < node.pathCost)
				return -1;
			if (pathCost > node.pathCost)
				return 1;
			return 0;
		}
	}

	public static List<StateWithCost> solve(StateWithCost start, StateWithCost goal) {
		// Prioritized collection of nodes that have not been explored.
		PriorityQueueSet<Node> frontier = new PriorityQueueSet<>();
		// Collection (with fast search) holding nodes that have been explored.
		Set<StateWithCost> explored = new HashSet<>();
		// Will hold the solution path, if found.
		List<StateWithCost> solution = null;
		// The total number of nodes explored.
		int nodesExplored = 0;
		// The total number of nodes that have been in the frontier.
		int nodesInFrontier = 0;
		// The total number of nodes created.
		int nodesCreated = 0;

		Node node = new Node(start, null, 0);
		frontier.add(node);
		nodesInFrontier++;
		while (!frontier.isEmpty()) {
			
			node = frontier.remove();
			nodesExplored++;
			
			if (node.state.equals(goal)) {
				
				solution = constructSolutionPath(node);
				break;
			}
			
			explored.add(node.state);
			
			Map<? extends StateWithCost,Double> neighbors = node.state.getNeighborsWithCost();
			
			for (StateWithCost child : neighbors.keySet()) {
				
				nodesCreated++;
				
				if (!explored.contains(child)) {
					
					Node childNode = new Node(child, node, node.pathCost + neighbors.get(child));
					frontier.add(childNode);
					nodesInFrontier++;
				}
			}
		}
		if (solution != null) {
			for (StateWithCost state : solution)
				System.out.println(state);
			System.out.println("Solution in " + (solution.size() - 1) + " moves.");
			System.out.println("Solution cost: " + node.pathCost);
		}
		System.out.println("   Nodes explored: " + nodesExplored);
		System.out.println("Nodes in frontier: " + nodesInFrontier);
		System.out.println("    Nodes created: " + nodesCreated);
		return solution;
	}
	
	private static List<StateWithCost> constructSolutionPath(Node node) {
		
		LinkedList<StateWithCost> path = new LinkedList<>();
		while (node != null) {
			path.addFirst(node.state);
			node = node.parent;
		}
		return path;
	}
}
